#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <mc_common/base_define.h>
#include <mc_velocity_planning/velocity_planning_base.h>
#include <mc_velocity_planning/dwa/dwa.h>


class TestDWA{
    public:
        TestDWA(tf2_ros::Buffer* tf_buffer);

        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);

         // 循环控制
        void Loop();
    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber fusionDataSub_;
        ros::Publisher LookaheadPosePub_;
        boost::shared_ptr<DWA> velocityPlannerPtr_; 
        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;
        boost::shared_ptr<geometry_msgs::PoseStamped> rvizGoalPtr_;
        
};

TestDWA::TestDWA(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer),
    fusionDataPtr_(nullptr), rvizGoalPtr_(nullptr){
    velocityPlannerPtr_ = boost::make_shared<DWA>(tf_buffer);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestDWA::RvizPosesCallback, this);
    fusionDataSub_ = nh_.subscribe("/fusion_cloud", 10, &TestDWA::FusionDataCallback, this);
    LookaheadPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);

    // 循环控制
    Loop();
}


// Rviz位姿回调函数
void TestDWA::RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg){
    LOG(INFO) << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    rvizGoalPtr_ = msg;
}

// 融合数据回调函数
void TestDWA::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;
    velocityPlannerPtr_->setPointCloudData(fusionDataPtr_);
}

// 循环控制
void TestDWA::Loop(){
    ros::Rate controller_rate(20);  //20Hz
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        if(!rvizGoalPtr_) continue;
        // 获取机器人当前位姿
        geometry_msgs::PoseStamped robotPoseStamp;
        if(!velocityPlannerPtr_->GetRobotPose(robotPoseStamp)){
            LOG(WARNING) << "can not get robot pose!";
            continue;
        }
        Pose2D robotPose2dGlobal = msg_convert::ToPose2D(robotPoseStamp.pose);

        Velocity2D curr_vel;
        Velocity2D next_vel;
        Pose2D goalPose2dGlobal = msg_convert::ToPose2D(rvizGoalPtr_->pose);
        Pose2D goalPose2dLocal = math_utils::Pose2DGlobal2Local(robotPose2dGlobal, goalPose2dGlobal);

        // 发布局部目标点
        geometry_msgs::PoseStamped lookahead_pose 
                = msg_convert::ToGeometryPoseStamp(goalPose2dLocal, "base_link");
        LookaheadPosePub_.publish(lookahead_pose);

        Pose2D robotPose2dLocal;
        velocityPlannerPtr_->ComputeTwist(curr_vel, next_vel, goalPose2dLocal);
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;    // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "test_dwa_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_dwa_node start...");

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestDWA test_dwa(&tfBuffer);
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}
